
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  ** This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * COPYRIGHT(c) 2018 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,
  * are permitted provided that the following conditions are met:
  *   1. Redistributions of source code must retain the above copyright notice,
  *      this list of conditions and the following disclaimer.
  *   2. Redistributions in binary form must reproduce the above copyright notice,
  *      this list of conditions and the following disclaimer in the documentation
  *      and/or other materials provided with the distribution.
  *   3. Neither the name of STMicroelectronics nor the names of its contributors
  *      may be used to endorse or promote products derived from this software
  *      without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f1xx_hal.h"
#include "adc.h"
#include "can.h"
#include "tim.h"
#include "gpio.h"

/* USER CODE BEGIN Includes */
#include "stm32f1xx_hal_adc.h"
#include "spi.h"
#include "stdarg.h"
#include "stdio.h"
/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */

uint16_t A_Pin[] = {A1_Pin, A2_Pin, A3_Pin, A4_Pin, A5_Pin, A6_Pin, A7_Pin, A8_Pin};

uint8_t FLAG_10MS = 0;
uint8_t FLAG_2S = 0;

int Count50ms = 0;
int Count2s = 0;

volatile unsigned long Time_Delay;

static CanTxMsgTypeDef TxMsg;
static CanRxMsgTypeDef RxMsg;

uint16_t MOTOR_ID = 0x00;
int Sport_Mode = 0;

int Set_low = 40;
int Set_high = 100;
int Set_Power = 30; //kg

int point_low = 0;  //(int)(Set_low - MechanicalHeight) * CoefficientC2E;
int point_high = 0; //(int)(Set_high - MechanicalHeight) * CoefficientC2E;
int height = 0;
int get_height = 0;

int adc = 0;
int Power = 300;
int Power_Value;
int encode = 0;
int encode_correction = 0;

uint8_t L_MOTOR = 0;
uint8_t R_MOTOR = 0;

uint8_t FLAG_EMERGENCY_BRAKING = 0;
uint8_t FLAG_START = 0;
uint8_t FLAG_STOP = 1;
uint8_t FLAG_BACE = 0;
uint8_t FLAG_CAN = 0;

int L_Height, R_Height;
int Dis_O, Dis_I;

S_s flag = F_NONE;
S_m mode = M_NONE;
S_e error = E_NONE;

uint8_t Tick_Flag = 0;

/* Private variables ---------------------------------------------------------*/
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);

/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/
void SetACS(ACS_t cs)
{
  if (cs == ACS_1)
  {
    HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
  }
  else if (cs == ACS_2)
  {
    HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_RESET);
  }
  else
  {
    HAL_GPIO_WritePin(CS1_GPIO_Port, CS1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(CS2_GPIO_Port, CS2_Pin, GPIO_PIN_SET);
  }
}

/* USER CODE END PFP */

/* USER CODE BEGIN 0 */
void mcpy(uint8_t *des, const uint8_t *src, uint32_t len)
{
  for (int i = 0; i < len; ++i)
  {
    des[i] = src[i];
  }
}

//Can config filter with shield mode
void CAN_Config()
{
  hcan.pTxMsg = &TxMsg;
  hcan.pRxMsg = &RxMsg;

  CAN_FilterConfTypeDef cFilterConfig;
  cFilterConfig.FilterNumber = 0;
  cFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK;
  cFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT;
  cFilterConfig.FilterIdHigh = 0x0000;
  cFilterConfig.FilterIdLow = 0x0000;
  cFilterConfig.FilterMaskIdHigh = 0x0000;
  cFilterConfig.FilterMaskIdLow = 0x0000;
  cFilterConfig.FilterFIFOAssignment = CAN_FIFO0;
  cFilterConfig.FilterActivation = ENABLE;
  cFilterConfig.BankNumber = 14;
  HAL_CAN_ConfigFilter(&hcan, &cFilterConfig);
}

//tick ms
bool Tick_Init()
{
  if (SysTick_Config(HAL_RCC_GetHCLKFreq() / 1000))
    return false;
  Time_Delay = TICK_DELAY;
  return true;
}

void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{

  if (htim->Instance == htim1.Instance)
  {
    FLAG_10MS = 1;
  }
}

bool TestLinker()
{
  //tension sensor link
  HAL_ADC_Start(&hadc1);
  HAL_ADC_PollForConversion(&hadc1, 0x10);

  int32_t adc = HAL_ADC_GetValue(&hadc1) * 3300 / 4095 - 1650;

  if (abs(adc) > 1000)
  {
    SetA(DI1, 0);
    OLED_Print(0, 6, "tension sensor");
    return false;
  }

  int adc_dis[2] = {0};
  for (int i = 0; i < 2; i++)
  {
    HAL_ADC_Start(&hadc2);
    HAL_ADC_PollForConversion(&hadc2, 0xffff);
    adc_dis[i] = HAL_ADC_GetValue(&hadc2) * 3300 / 4095;
  }

  if (abs(adc_dis[0]) > 1000 || abs(adc_dis[1]) > 1000)
  {
    SetA(DI1, 0);
    OLED_Print(0, 6, "distance sensor");
    return false;
  }

  return true;
}

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  *
  * @retval None
  */
int main(void)
{
  /* USER CODE BEGIN 1 */
  /* USER CODE END 1 */

  /* MCU Configuration----------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_CAN_Init();
  MX_TIM3_Init();
  MX_TIM2_Init();
  MX_ADC1_Init();
  MX_ADC2_Init();
  MX_TIM1_Init();
  /* USER CODE BEGIN 2 */

  AD5724R_Init();
  AD5724R_SetPower(AD5724R_DAC_ALL);
  AD5724R_RangeSelect();
  AD5724R_LoadDac(AD5724R_DAC_A, 0); //-2048 -  2047

  Tick_Init();
  OLED_Init();

  TroubleLED(0);
  NormalLED(0);

  CAN_Config();
  HAL_CAN_Receive_IT(&hcan, CAN_FIFO0);
  HAL_NVIC_SetPriority(USB_LP_CAN1_RX0_IRQn, 1, 0);
  HAL_NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn);
  HAL_CAN_RxCpltCallback(&hcan);
  HAL_ADCEx_Calibration_Start(&hadc1);
  HAL_ADCEx_Calibration_Start(&hadc2);
  HAL_TIM_Encoder_Start_IT(&htim3, TIM_CHANNEL_ALL);
  HAL_TIM_Base_Init(&htim1);
  HAL_TIM_Base_Start_IT(&htim1);

  Sport_Mode = HARDPULL_MODE;
  OLED_Print(0, 0, "*****PE FITNESS*****");
  OLED_Printf(0, 1, "MODE:%d", Sport_Mode);

  //	while(!TestLinker());

  SetACS(ACS_1);
  SetA(DI1, 1);

  if (!HAL_GPIO_ReadPin(MID_GPIO_Port, MID_Pin))
  {
    L_MOTOR = 1;
    R_MOTOR = 0;
  }
  else if (HAL_GPIO_ReadPin(MID_GPIO_Port, MID_Pin))
  {
    L_MOTOR = 0;
    R_MOTOR = 1;
  }
  else
  {
    SetA(DI1, 0);
    Error_Handler();
  }

  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  int _encode = 0, _adc = 0;
  int adc_dis[2] = {0};
  int speed = 0;

  Set_low = 40;
  Set_high = 100;
  Set_Power = 30;
  Power = 400;

  switch (Sport_Mode)
  {
  case HARDPULL_MODE:
  {
    Set_low = 40;
    Set_high = 70;
  }
  break;
  case DEEPSQUART_MODE:
  {
    Set_low = 80;
    Set_high = 140;
  }
  break;
  case HIGHDROP_MODE:
  {
    Set_low = 50;
    Set_high = 150;
  }
  break;
  case ROW_MODE:
  {
    Set_low = 50;
    Set_high = 70;
  }
  break;
  }

  point_low = (int)(Set_low - MechanicalHeight) * CoefficientC2E;
  point_high = (int)(Set_high - MechanicalHeight) * CoefficientC2E;

  encode_correction = Start_SelfTest();

  if (L_MOTOR)
  {
    MOTOR_ID = L_MOTOR_ID;

    switch (Sport_Mode)
    {
    case DEEPSQUART_MODE:
      //Power += 50;
      break;
    case HIGHDROP_MODE:
      Power -= 25;
      break;
    }
  }
  if (R_MOTOR)
  {
    MOTOR_ID = R_MOTOR_ID;

    switch (Sport_Mode)
    {
    case DEEPSQUART_MODE:
      //	Power -= 25;
      break;
    case HIGHDROP_MODE:
      Power += 50;
      break;
    }
  }

  Power_Value = Power;
  //	static int cnt_CanError ;
  while (1)
  {

    if (FLAG_10MS)
    {

      SovleCanErr();
      //			if(FLAG_CAN != HAL_OK)
      //			{
      //				cnt_CanError ++;
      //				HAL_CAN_Receive_IT(&hcan, CAN_FIFO0);
      //				if(cnt_CanError > 10)
      //				{
      //					FLAG_EMERGENCY_BRAKING = 1;
      //					CAN_LoadDataFrame(MOTOR_ID, CMD_SAFE, 0x0001);
      //					HAL_CAN_Transmit_IT(&hcan);
      //				}

      //			 }

      ModeChange();

      if (FLAG_2S)
        Count2s++;
      else
        Count2s = 0;

      FLAG_10MS = 0;

      HAL_ADC_Start(&hadc1);
      HAL_ADC_PollForConversion(&hadc1, 0x10);

      adc = HAL_ADC_GetValue(&hadc1) * 3300 / 4095 - 1650;
      _adc = adc = (adc * 8 + _adc * 2) / 10;
      /* USER CODE END WHILE */

      /* USER CODE BEGIN 3 */

      for (int i = 0; i < 2; i++)
      {
        HAL_ADC_Start(&hadc2);
        HAL_ADC_PollForConversion(&hadc2, 0xffff);
        adc_dis[i] = HAL_ADC_GetValue(&hadc2) * 3300 / 4095;
      }
      Dis_I = adc_dis[0];
      Dis_O = adc_dis[1];

      if (L_MOTOR)
        encode = -(int16_t)(htim3.Instance->CNT);
      if (R_MOTOR)
        encode = (int16_t)(htim3.Instance->CNT);

      encode -= encode_correction;

      speed = (encode - _encode) * (100 * 60) / (256 * 4);
      _encode = encode;

      height = CoefficientE2C * encode;

      int *pNew = DisPID();
      int err = *pNew;
      int analog = *(pNew + 1);
      if (!get_height)
      {
        Analog(analog, FLAG_LOWSP);
        if (abs(err) < 200)
          get_height = 1;
      }
      else
      {
        switch (Sport_Mode)
        {
        case STOP_MODE:
          SetState();
          break;
        case HARDPULL_MODE:
          HardPull(err, analog);
          break;
        case DEEPSQUART_MODE:
        {
          if (L_MOTOR)
            DeepPowerPID(R_Height);
          if (R_MOTOR)
            DeepPowerPID(L_Height);
          DeepSquart(err, analog, speed);
        }
        break;
        case HIGHDROP_MODE:
        {

          if (L_MOTOR)
            HDropPowerPID(R_Height, 50, 50);
          if (R_MOTOR)
            HDropPowerPID(L_Height, 50, 50);
          HighDrop(err, analog, speed);
        }
        break;
        case ROW_MODE:
          RowMode(err, analog);
          break;
        }
      }

      //      if (L_MOTOR)
      //      {
      //        if (Count50ms++ > 5)
      //        {
      //          Count50ms = 0;
      //          CAN_LoadDataFrame(L_MOTOR_ID, CMD_HEIGHT, height);
      //          HAL_CAN_Transmit_IT(&hcan);

      //          HeightCheck(height,R_Height);
      //        }
      //      }

      //      if (R_MOTOR)
      //      {
      //          if (Count50ms++ > 5)
      //          {
      //						Count50ms = 0;
      //            CAN_LoadDataFrame(R_MOTOR_ID, CMD_HEIGHT, height);
      //            HAL_CAN_Transmit_IT(&hcan);

      //            HeightCheck(height,L_Height);
      //          }
      //      }
      if (Count50ms++ > 5)
      {
        Count50ms = 0;
        CAN_LoadDataFrame(MOTOR_ID, CMD_HEIGHT, height);
        HAL_CAN_Transmit_IT(&hcan);
        if (L_MOTOR)
          HeightCheck(height, R_Height);
        if (R_MOTOR)
          HeightCheck(height, L_Height);
      }

      if (FLAG_EMERGENCY_BRAKING)
      {
        SetA(DI1, 0);
        FLAG_EMERGENCY_BRAKING = 0;
      }

      OLED_Printf(0, 1, "MODE:%d", Sport_Mode);
      OLED_ErrorLog(0, 7);

      //      OLED_Printf(0, 1, "%3d %3d %3d %3d", Dis_I, Dis_O, Set_low, Set_high);
      OLED_Printf(0, 2, "%5d  %5d %5d", height, R_Height, L_Height);
      OLED_Printf(0, 3, "%5d %d", encode, analog);
      //      OLED_Printf(0, 4, "0x%x :%x :%x", datRx.CanId, datRx.Cmd, datRx.Content);
      OLED_Printf(0, 5, "%d %d %d %d %d", FLAG_START, FLAG_CAN, flag, Power, FLAG_EMERGENCY_BRAKING);
    }
  }

  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{

  RCC_OscInitTypeDef RCC_OscInitStruct;
  RCC_ClkInitTypeDef RCC_ClkInitStruct;
  RCC_PeriphCLKInitTypeDef PeriphClkInit;

  /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_ADC;
  PeriphClkInit.AdcClockSelection = RCC_ADCPCLK2_DIV6;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  /**Configure the Systick interrupt time 
    */
  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq() / 1000);

  /**Configure the Systick 
    */
  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}

/* USER CODE BEGIN 4 */

void HAL_SYSTICK_Callback(void)
{
  if (Time_Delay)
  {
    Time_Delay--;
  }
  else
  {
    SysTick->VAL = 0x00;
    Time_Delay = TICK_DELAY;
    Tick_Flag = 1;
  }
}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @param  file: The file name as string.
  * @param  line: The line in file as a number.
  * @retval None
  */
void _Error_Handler(char *file, int line)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  while (1)
  {
    OLED_Clear();
    OLED_Print(0, 0, "Error!");
    OLED_Printf(0, 1, "#%s", &file[3]);
    OLED_Printf(0, 2, "@%d", line);
    OLED_Printf(0, 3, "%d", HAL_CAN_Transmit(&hcan, 0xff));
    while (1)
    {
      HAL_GPIO_TogglePin(TroubleLED_GPIO_Port, TroubleLED_Pin);
      HAL_Delay(200);
    }
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  OLED_Clear();
  OLED_Print(0, 0, "Assert failed!");
  OLED_Printf(0, 1, "#%s", &file[3]);
  OLED_Printf(0, 2, "@%d", line);
  while (1)
  {
    HAL_GPIO_TogglePin(TroubleLED_GPIO_Port, TroubleLED_Pin);
    HAL_Delay(500);
  }
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
